﻿using System;
using System.Collections.Generic;
using System.Diagnostics;

namespace NestedParticleFilter
{
    public class Particle : ICloneable
    {
        // how many particles are made
        private static int _count;

        // Class Members
        private RobotPose _pose; // pose of the Particle
        private double _weight; // weight of the particle
        // nested particle specific members
        private int _numOfNestedParticle;
        private List<Particle> _nestedParticles;
        private double _leftoverTime;       // the left over time in milliseconds for an action to be perfomred for a specific nested particle
        private Motions _leftoverMotion;    //the left over motion to be perfomred for a specific nested particle
        private bool _isTagged;             // flag to be set when a nested particle is tagged
        private bool _isDone;               // flag to be set when a nested particle is done pushing a landmark
        private bool _isTurned;             // flag to be set when a nested particle is turned toward a landmark
        private bool _isReached;            // flag to be set when a nested particle is reached a landmark
        private Landmark _landmarkToPush;   // a landmark to be pushed by a nested particle
        private short _landmarkIndex;       // index of a landmark that a nested particle will push
        private bool _isMapUpdated;         // flag to be set when a map is updated based on the particle's position

        private bool _pushed;
        #region Constructors
        /// <summary>
        /// Constructor
        /// </summary>
        /// <param name="num">number of nested particles. If 0, the particle won't be having nested particles</param>
        /// <exception cref="ArgumentOutOfRangeException"><c>num</c> is out of range.</exception>
        public Particle(int num)
        {
            if (num < 0 )
                throw new ArgumentOutOfRangeException("num","Parameter cannot be negative");
            _pose = new RobotPose(0, 0, 0);
            _weight = 1;
            _numOfNestedParticle = num;
            _nestedParticles = num!= 0 ? new List<Particle>() : null;
            _leftoverTime = 0;
            _leftoverMotion = null;
            _isDone = false;
            _isTurned = false;
            _isTagged = false;
            _isReached = false;
            _isMapUpdated = false;
            _pushed = false;
            _landmarkIndex = -1;
            _landmarkToPush = null;
            _count++;
        }

        /// <summary>
        /// Constructor
        /// </summary>
        /// <param name="pose">location of the particle</param>
        /// <param name="weight">weight of the particle</param>
        /// <param name="num">number of nested particles</param>
        /// <exception cref="ArgumentException">Weight should be >= 0 and &alt;= 1</exception>
        /// <exception cref="ArgumentNullException"><paramref name="pose" /> is <c>null</c>.</exception>
        /// <exception cref="ArgumentOutOfRangeException"><c>num</c> is out of range.</exception>
        public Particle(RobotPose pose, double weight, int num)
        {
            //if (weight < 0 || weight > 1)
            //    throw new ArgumentException("Weight should be >= 0 and <= 1", "weight");
            if (pose == null)
                throw new ArgumentNullException("pose", "Robot's Pose should not be null");
            if (num < 0)
                throw new ArgumentOutOfRangeException("num", "Parameter cannot be negative");
            _pose = pose;
            _weight = weight;
            _nestedParticles = num != 0 ? new List<Particle>() : null;
            _leftoverTime = 0;
            _leftoverMotion = null;
            _isDone = false;
            _isTurned = false;
            _isTagged = false;
            _isReached = false;
            _isMapUpdated = false;
            _pushed = false;
            _landmarkIndex = -1;
            _landmarkToPush = null;
            _count++;
        }

        /// <summary>
        /// Constructor
        /// </summary>
        /// <param name="pose">location of the particle</param>
        /// <param name="weight">weight of the particle</param>
        /// <param name="num">number of nested particles</param>
        /// <exception cref="ArgumentException">Weight should be >= 0 and &alt;= 1</exception>
        /// <exception cref="ArgumentNullException"><paramref name="pose" /> is <c>null</c>.</exception>
        /// <exception cref="ArgumentOutOfRangeException"><c>num</c> is out of range.</exception>
        /// <exception cref="Exception">number of nested particles must match the size of the list</exception>
        public Particle(RobotPose pose, double weight, int num, List<Particle> newList)
        {
            //if (weight < 0 || weight > 1)
            //    throw new ArgumentException("Weight should be >= 0 and <= 1", "weight");
            if (pose == null)
                throw new ArgumentNullException("pose", "Robot's Pose should not be null");
            if (num < 0)
                throw new ArgumentOutOfRangeException("num", "Parameter cannot be negative");
            if (newList != null && num != newList.Count)
                throw new Exception("number of nested particles must match the size of the list");
            _pose = pose;
            _weight = weight;
            _nestedParticles = num != 0 ? newList : null;
            _leftoverTime = 0;
            _numOfNestedParticle = num;
            _leftoverMotion = null;
            _isDone = false;
            _isTurned = false;
            _isTagged = false;
            _isReached = false;
            _isMapUpdated = false;
            _pushed = false;
            _landmarkIndex = -1;
            _landmarkToPush = null;
            _count++;
        }
        #endregion 

        #region Getters and Setters

        /// <summary>
        /// Pose of the particle
        /// </summary>
        public RobotPose Pose
        {
            set { _pose = value; }
            get { return _pose; }
        }

        public double Weight
        {
            set
            {
                //if (value < 0)
                //    throw new ArgumentException("weight out of range");
                _weight = value;
            }
            get
            {
                return this._weight;
            }
        }

        /// <summary>
        /// Gets and sets the nested particles list
        /// </summary>
        public List<Particle> NestedParticles
        {
            set { if (value != null) _nestedParticles = value; }
            get { return this._nestedParticles; }
        }
        
        /// <summary>
        /// The left over time in milliseconds for a nested particle  to finish an action.
        /// </summary>
        /// <exception cref="ArgumentException">Time should not be negative</exception>
        public  double LeftoverTime
        {
            set
            {
                if (value < 0)
                    throw new ArgumentException("Time should not be negative");
                _leftoverTime = value;
            }
            get { return _leftoverTime; }
        }
       
        /// <summary>
        /// Previous motion action that the particle performed
        /// </summary>
        public Motions LeftoverMotion
        {
            set { _leftoverMotion = value; }
            get { return _leftoverMotion; }
        }

        /// <summary>
        /// if the particle is tagged
        /// </summary>
        public bool IsTagged
        {
            set { _isTagged = value; }
            get { return _isTagged; }
        }
        /// <summary>
        /// The particle is done pushing the landmark
        /// </summary>
        public bool IsDone
        {
            set { _isDone = value; }
            get { return _isDone; }
        }
        /// <summary>
        /// The particle is turned toward the landmark
        /// </summary>
        public bool IsTurned
        {
            set { _isTurned = value; }
            get { return _isTurned; }
        }
        /// <summary>
        /// The particle is reached the landmark
        /// </summary>
        public bool IsReached
        {
            set { _isReached = value; }
            get { return _isReached; }
        }
        
        public bool IsMapUpdated
        {
            set { _isMapUpdated = value; }
            get { return _isMapUpdated; }
        }
        public bool Pushed
        {
            set { _pushed = value; }
            get { return _pushed; }
        }

        /// <summary>
        /// Closest landmark to the particle
        /// </summary>
        public Landmark LandmarkToPush
        {
            set { _landmarkToPush = value; }
            get { return _landmarkToPush; }
        }
        /// <summary>
        /// Index of the closest landmark
        /// </summary>
        public short LandmarkIndex
        {
            set { _landmarkIndex = value; }
            get { return _landmarkIndex; }
        }

        #endregion

        public override string ToString()
        {
            return Pose + "\tTagged: " + _isTagged + " Turned: " + IsTurned + " Reached: " + IsReached + " Done: " +
                   IsDone;
        }

        #region ICloneable Members

        /// <summary>
        /// Clones the particle
        /// </summary>
        /// <returns>New referance of the current particle</returns>
        public object Clone()
        {
            var clonedPose = new RobotPose(_pose.X, _pose.Z, _pose.Theta);            
            double weight = _weight;
            int num = _numOfNestedParticle;
            List<Particle> newList = new List<Particle>();
            if (_nestedParticles != null)
                for (int i = 0 ; i < _nestedParticles.Count; i++)
                    newList.Add((Particle) _nestedParticles[i].Clone());
            
            Debug.Assert(num==newList.Count," They should have the same size...."+ num + " list size: " + newList.Count);
            var copy = new Particle(clonedPose, weight,num,newList);
            
            copy._leftoverTime = _leftoverTime;
            if (_leftoverMotion != null)
                copy.LeftoverMotion = new Motions(_leftoverMotion.Velocity,_leftoverMotion.Omega);
            copy.IsDone = _isDone;
            copy.IsTagged = _isTagged;
            copy.IsTurned = _isTurned;
            copy.IsReached = _isReached;
            copy.IsMapUpdated = _isMapUpdated;
            copy.Pushed = _pushed;
            copy.LandmarkIndex = _landmarkIndex;
            if (_landmarkToPush != null)
                copy.LandmarkToPush = (Landmark) _landmarkToPush.Clone();
            return copy;
        }

        #endregion


        #region Static Methods
        /// <summary>
        /// Generates a random pose that is valid inside the map
        /// </summary>
        /// <param name="seed">seed value</param>
         /// <returns>random pose</returns>
        private static RobotPose GenerateRandomPose(int seed)
        {
            var random = new Random(seed*_count);
            int num = random.Next() % 2;
            double x = 0, z = 0;
            switch (num)
            {
                case 0:
                    x = random.NextDouble() * Map.GridWidth + Map.XMinCorner; // A double number between [-14.0,14.7]
                    if (x >= Map.XMinHallCorner && x <= Map.XMaxHallCorner)
                        z = random.NextDouble() * Map.GridHeight + Map.ZMinCorner; //  A double number between [-8.7,8.3]
                    else if (x >= Map.XMinCorner && x <= Map.XMaxCorner)
                        z = random.NextDouble() * (Map.ZMaxHallCorner - Map.ZMinHallCorner) + Map.ZMinHallCorner;
                    // A double number between [5.52 ,8.3]
                    break;
                case 1:
                    z = random.NextDouble() * Map.GridHeight + Map.ZMinCorner;
                    if (z >= Map.ZMinHallCorner && z <= Map.ZMaxHallCorner)
                        x = random.NextDouble() * Map.GridWidth + Map.XMinCorner; // A double number between [-14.3,14.7]
                    else if (z >= Map.ZMinCorner && z <= Map.ZMinHallCorner)
                        x = random.NextDouble() * (Map.XMaxHallCorner - Map.XMinHallCorner) + Map.XMinHallCorner;
                    // double number between [ 8.3,11.3]
                    break;
            }
            double theta = random.Next(-180, 180);
            var pose = new RobotPose(x, z, theta);
            _count++;
            if (!IsValidParticle(pose))
                PutParticlePoseInMap(pose);
            return pose;
        }

        /// <summary>
        /// Generates a random pose that is valid inside the map with inital theta as parameter
        /// </summary>
        /// <param name="seed">seed value</param>
        /// <param name="theta"> theta of the particle</param>
        /// <returns>random pose</returns>
        private static RobotPose GenerateRandomPose(int seed, double theta)
        {
            var random = new Random(seed*_count);
            int num = random.Next()%2;
            double x = 0, z = 0;
            switch (num)
            {
                case 0:
                    x = random.NextDouble()*Map.GridWidth + Map.XMinCorner; // A double number between [-14.0,14.7]
                    if (x >= Map.XMinHallCorner && x <= Map.XMaxHallCorner)
                        z = random.NextDouble()*Map.GridHeight + Map.ZMinCorner; //  A double number between [-8.7,8.3]
                    else if (x >= Map.XMinCorner && x <= Map.XMaxCorner)
                        z = random.NextDouble()*(Map.ZMaxHallCorner - Map.ZMinHallCorner) + Map.ZMinHallCorner;
                            // A double number between [5.52 ,8.3]
                    break;
                case 1:
                    z = random.NextDouble()*Map.GridHeight + Map.ZMinCorner;
                    if (z >= Map.ZMinHallCorner && z <= Map.ZMaxHallCorner)
                        x = random.NextDouble()*Map.GridWidth + Map.XMinCorner; // A double number between [-14.3,14.7]
                    else if (z >= Map.ZMinCorner && z <= Map.ZMinHallCorner)
                        x = random.NextDouble()*(Map.XMaxHallCorner - Map.XMinHallCorner) + Map.XMinHallCorner;
                            // double number between [ 8.3,11.3]
                    break;
            }

            var pose = new RobotPose(x, z, theta);
            _count++;
            if (!IsValidParticle(pose))
            {
                Debug.WriteLine("[GenerateRandomPose]: Did not generate a valid particle...");
                PutParticlePoseInMap(pose);
            }
            return pose;
        }

        /// <summary>
        /// generate a random pose in the map
        /// </summary>
        /// <param name="seed">seed of the rand function</param>
        /// <param name="theta">theta of the robot</param>
        /// <param name="verticalHall">percentage of particles in the vericall hall</param>
        /// <param name="horizontalHall">percentage of particles in the horizontal hall</param>
        /// <returns></returns>
        private static RobotPose GenratePercentagePose(int seed, double theta, double horizontalHall, double verticalHall)
        {
            var random = new Random(seed * _count);
            double num = random.NextDouble();
            double x = 0, z = 0;
            // Debug.WriteLine("num: "+ num);
            if (num <= verticalHall) // generate particle in the horizontal hall
            {
                x = random.NextDouble() * Map.GridWidth + Map.XMinCorner; // A double number between [-2.8,14.07]
                z = random.NextDouble() * (Map.ZMaxHallCorner - Map.ZMinHallCorner) + Map.ZMinHallCorner; // A double number between [5.52 ,8.3]

            }
            else
            {
                z = random.NextDouble() * (Map.ZMinHallCorner- Map.ZMinCorner) + Map.ZMinCorner;
                x = random.NextDouble() * (Map.XMaxHallCorner - Map.XMinHallCorner) + Map.XMinHallCorner; // double number between [ 8.3,11.3]
               
            }
            
            var pose = new RobotPose(x, z, theta);
            _count++;
           
            return pose;
        }

        /// <summary>
        /// Generate list of nested particles uniformally with a random theta for j's particles
        /// </summary>
        /// <param name="numberOfParticles">number of particles</param>
        /// <param name="initalTheta">inital theta of each particle</param>
        /// <param name="numOfNestedParticles">number of nested particles each particle has</param>
        /// <returns>list of nested particles</returns>
        public static List<Particle> UniformParticleInitialization(int numberOfParticles, double initalTheta, int numOfNestedParticles)
        {
            if (numberOfParticles == 0)
                return null;
            Random random = new Random();
            var particles = new List<Particle>();
            for (int i = 0; i < numberOfParticles; i++)
            {
                int nestedTheta = random.Next(-180, 180);
                var nested = UniformParticleInitialization(numOfNestedParticles, nestedTheta, 0);
                RobotPose p = GenerateRandomPose(DateTime.Now.Millisecond*i, initalTheta);
                var particle = new Particle(p, 1, numOfNestedParticles, nested);
                particles.Add(particle);
            }
            return particles;
        }

        /// <summary>
        /// Generate list of nested particles uniformally with a random theta for j's particles
        /// </summary>
        /// <param name="numberOfParticles">number of particles</param>
        /// <param name="initalTheta">inital theta of each particle</param>
        /// <param name="numOfNestedParticles">number of nested particles each particle has</param>
        /// <param name="initialJTheta">inital theta for particles of j</param>
        /// <returns>list of nested particles</returns>
        public static List<Particle> UniformParticleInitialization(int numberOfParticles, double initalTheta, int numOfNestedParticles, double initialJTheta)
        {
            if (numberOfParticles == 0)
                return null;
           var particles = new List<Particle>();
            for (int i = 0; i < numberOfParticles; i++)
            {
                var nested = UniformParticleInitialization(numOfNestedParticles, initialJTheta, 0);
                RobotPose p = GenerateRandomPose(DateTime.Now.Millisecond , initalTheta);
                var particle = new Particle(p, 1, numOfNestedParticles, nested);
                particles.Add(particle);
            }
            return particles;
        }

        /// <summary>
        /// Generate particles based on the distribution of vertical and horizontal halls
        /// </summary>
        /// <param name="numOfParticles">number of particles</param>
        /// <param name="initalTheta">intial theta in degree</param>
        /// <param name="numOfNestedParticles">Number of nested particles</param>
        /// <param name="verticalHall">percentate of particles in the verticall hallway</param>
        /// <param name="horizontalHall">percentage of particles in the horizontal hallway</param>
        /// <returns></returns>
        public static List<Particle> NonUniformParticleInitialization(int numOfParticles, double initalTheta, int numOfNestedParticles,
                                                                      double verticalHall, double horizontalHall)
        {
            if (numOfParticles == 0)
                return null;
            var particles = new List<Particle>();
           
            for (int i = 0; i < numOfParticles; i++)
            {
                Random random = new Random(DateTime.Now.Millisecond);
                double initialJTheta = random.Next(-180, 180);
                var nested = NonUniformParticleInitialization(numOfNestedParticles, initialJTheta, 0,0,1);
                RobotPose p = GenratePercentagePose(DateTime.Now.Millisecond, initalTheta, verticalHall,
                                                    horizontalHall);
                var particle = new Particle(p, 1, numOfNestedParticles, nested);
                particles.Add(particle);
            }
            return particles;
        }

        /// <summary>
        /// Generate particles based on the distribution of vertical and horizontal halls
        /// </summary>
        /// <param name="numOfParticles">number of particles</param>
        /// <param name="initalTheta">intial theta in degree</param>
        /// <param name="numOfNestedParticles">Number of nested particles</param>
        /// <param name="jTheta">theta of the opponent in degree</param>
        /// <param name="verticalHall">percentate of particles in the verticall hallway</param>
        /// <param name="horizontalHall">percentage of particles in the horizontal hallway</param>
        /// <returns></returns>
        public static List<Particle> NonUniformParticleInitialization(int numOfParticles, double initalTheta, int numOfNestedParticles, double jTheta,
                                                                      double verticalHall, double horizontalHall)
        {
            if (numOfParticles == 0)
                return null;
            var particles = new List<Particle>();

            for (int i = 0; i < numOfParticles; i++)
            {
                var nested = NonUniformParticleInitialization(numOfNestedParticles, jTheta, 0,0,0.5,0.5);
                RobotPose p = GenratePercentagePose(DateTime.Now.Millisecond, initalTheta, verticalHall,
                                                    horizontalHall);
                var particle = new Particle(p, 1, numOfNestedParticles, nested);
                particles.Add(particle);
            }
            return particles;
        }
        /// <summary>
        /// generates set of particles around the inital pose with no nested particles.
        /// </summary>
        /// <param name="initialPose"></param>
        /// <param name="numOfParticles"></param>
        /// <returns></returns>
        public static List<Particle> SpecificParticleInitialization(RobotPose initialPose,int numOfParticles)
        {
            var particles = new List<Particle>();
            for (int i = 0 ; i < numOfParticles; i++)
            {
                Random rand = new Random(i);
                double xerror = rand.NextDouble() * 0.4 - 0.2;

                double zerror = rand.NextDouble()*0.4 -0.2; // a number between -0.2 and 0.2
                double thetaError = rand.NextDouble()*0.4 -0.2;
                RobotPose robotPose = new RobotPose(initialPose.X+xerror, initialPose.Z + zerror, initialPose.Theta + thetaError);
                Particle particle = new Particle(robotPose,-1,0);
                particles.Add(particle);
                    
            }
            return particles;
        }
        #endregion

        #region Miscellaneous

        /// <summary>
        /// This function returns the mean squared error between the set of possible particles and the pose of a real robot
        /// </summary>
        /// <param name="particles">list of hypothesized location</param>
        /// <param name="realPose">real position</param>
        /// <returns>Mean squared error</returns>
        public static double MeanError(List<Particle> particles, RobotPose realPose)
        {
            if (particles == null)
                return -1;
            if (realPose == null)
                return -1;
            double sum = 0;
            for (int i = 0; i < particles.Count; i++)
            {
                double error =
                    Math.Sqrt(Math.Pow(realPose.X - particles[i]._pose.X, 2) +
                              Math.Pow(realPose.Z - particles[i]._pose.Z, 2));
                sum += error;
            }
            double mse = sum/particles.Count; // mean squared error
            return mse;
        }
       

        /// <summary>
        /// This function averages over all the poses of the particles
        /// </summary>
        /// <param name="particles">list of particles</param>
        /// <returns>averaged robot's pose</returns>
        public static RobotPose AveragedPose(List<Particle> particles)
        {
            //averaging over particle's pose
            double x = 0, z = 0, xtheta = 0, ztheta = 0, theta = 0;



            for (int i = 0; i < particles.Count; i++)
            {
                x += particles[i]._pose.X;
                z += particles[i]._pose.Z;
                xtheta += Math.Sin(UnitConversion.DegreeToRadian(particles[i]._pose.Theta));
                ztheta += Math.Cos(UnitConversion.DegreeToRadian(particles[i]._pose.Theta));
            }
            if (particles.Count != 0)
            {
                x /= particles.Count;
                z /= particles.Count;
            }
            theta = Math.Atan2(xtheta, ztheta);
            theta = UnitConversion.RadianToDegree(theta);
            return new RobotPose(x, z, theta);
        }

        #endregion

        #region Particle Validation For Map

        /// <summary>
        /// Returns true if the particle has the valid  pose
        /// </summary>
        /// <param name="p">particle's pose </param>
        /// <returns>true if the particle's pose is valid and false otherwise</returns>
        public static bool IsValidParticle(Point p)
        {
            if (p.Z >= Map.ZMinCorner && p.Z <= Map.ZMaxCorner)
            {
                if (p.X >= Map.XMinHallCorner && p.X <= Map.XMaxHallCorner)
                    return true;
            }
            if (p.Z >= Map.ZMinHallCorner && p.Z <= Map.ZMaxHallCorner)
            {
                if (p.X >= Map.XMinCorner && p.X <= Map.XMaxCorner)
                    return true;
            }
            return false;
        }

        /// <summary>
        /// returns true if a point has a valid x position in the map
        /// </summary>
        /// <param name="x">x position of a particle</param>
        /// <returns>true if the point's x is valid and false otherwise</returns>
        private static bool HasValidX(double x)
        {
            if (x >= Map.XMinCorner && x <= Map.XMaxCorner)
                return true;
            return false;
        }

        /// <summary>
        /// returns true if the particle's z position is valid in the map
        /// </summary>
        /// <param name="z">z position of the particle</param>
        /// <returns>true of z is valid and false otherwise</returns>
        private static bool HasValidZ(double z)
        {
            if (z >= Map.ZMinHallCorner && z <= Map.ZMaxHallCorner)
                return true;
            return false;
        }
        private static void PutThetaInPlace(RobotPose p)
        {
            if (p.Theta > 0 && p.Theta < 45)
                p.Theta = 0;
            if (p.Theta >= 45 && p.Theta < 135)
                p.Theta = 90;
            if (p.Theta >= 135)
                p.Theta = 180;

            if (p.Theta < 0 && p.Theta >= -45)
                p.Theta = 0;
            if (p.Theta < -45 && p.Theta >= -135)
                p.Theta = -90;
            if (p.Theta <= -135)
                p.Theta = -180;

        }

        /// <summary>
        /// if a particle has moved outside the boundaries of the map, the particle is 
        /// placed in to the closest border of its invalid location. This is for propagation
        /// purposes. Because otherwise the particle will die and will vanish from the map and we
        /// will have less number of particles
        /// </summary>
        /// <param name="p"></param>
        public static void PutParticlePoseInMap(RobotPose p)
        {

            // if particle is in A1 
            bool inArea1 = (p.X < Map.XMinHallCorner && p.X >= Map.XMinCorner && p.Z <= Map.ZMinHallCorner &&
                       p.Z >= Map.ZMinCorner);

            bool inArea2 = (p.X >Map.XMaxHallCorner && p.X <= Map.XMaxCorner && p.Z <= Map.ZMinHallCorner &&
                       p.Z >= Map.ZMinCorner);

            bool inArea3 = p.X > Map.XMaxCorner;
            bool inArea4 = p.Z > Map.ZMaxCorner;
             if (inArea2)
            {
                //Debug.WriteLine("in Area 2 " + p);
                double d1 = Math.Abs(p.Z - Map.ZMinHallCorner);
                double d2 = Math.Abs((p.X - Map.XMaxHallCorner));
                if (d1 <= d2)
                    p.Z = Map.ZMinHallCorner;
                else
                {
                    p.X = Map.XMaxHallCorner;
                }
            }
            else if (inArea1)
            {
                //Debug.WriteLine("in Area 1 "+ p);
                double d1 = Math.Abs(p.Z - Map.ZMinHallCorner);
                double d2 = Math.Abs(p.X - Map.XMinHallCorner);
                if (d1 <= d2)
                    p.Z = Map.ZMinHallCorner;
                else
                {
                    p.X = Map.XMinHallCorner;
                }
            }
             else if (inArea3)
             {
                 p.X = Map.XMaxCorner;
                 p.Z = Math.Abs(p.Z - Map.ZMinHallCorner) < Math.Abs(p.Z - Map.ZMaxHallCorner) ? Map.ZMinHallCorner : Map.ZMaxHallCorner;
             }
             else if (inArea4)
             {
                 p.Z = Map.ZMaxCorner;
             }
             else
             {
                 if (p.Z < Map.ZMinCorner)
                     p.Z = Map.ZMinCorner;
                 else if (p.Z > Map.ZMaxCorner)
                     p.Z = Map.ZMaxCorner;

                 if (p.X < Map.XMinCorner)
                     p.X = Map.XMinCorner;
                 else if (p.X > Map.ZMaxCorner)
                     p.X = Map.ZMaxCorner;

             }
            //// if the z location is valid
            //if (HasValidZ(p.Z))
            //{
            //    if (p.X < Map.XMinCorner)
            //        p.X = Map.XMinCorner;
            //    else if (p.X > Map.XMaxCorner)
            //        p.X = Map.XMaxCorner;
            //}
            //else if (HasValidX(p.X))
            //{
            //    if (p.Z < Map.ZMinCorner)
            //        p.Z = Map.ZMinCorner;
            //    if (p.Z > Map.ZMaxCorner)
            //        p.Z = Map.ZMaxCorner;
            //    if (!(p.X >= Map.XMinHallCorner && p.X <= Map.XMaxHallCorner)
            //        && (!HasValidZ(p.Z))) // x is not between x of horizontal hall 
            //    {
            //        if (p.X < Map.XMinHallCorner)
            //        {
            //            //find if p.z us closer to zmin hall or p.x is closer to xmin hall);
            //            if (Math.Abs(p.Z - Map.ZMinHallCorner) < Math.Abs(p.X - Map.XMinHallCorner))
            //                p.Z = Map.ZMinHallCorner;
            //            else
            //                p.X = Map.XMinHallCorner;
            //        }
            //        else if (p.X > Map.XMaxHallCorner)
            //        {
            //           //find if p.z us closer to zmin hall or p.x is closer to xmin hall
            //            double distance = Math.Abs(p.Z - Map.ZMinHallCorner);
            //            double distance1 = Math.Abs(p.X - Map.XMaxHallCorner);
                        
            //            double d = Math.Min(distance1, distance);
            //            if ( d == distance)
            //                p.Z = Map.ZMinHallCorner;
            //            else if ( d==distance1 )
            //                p.X = Map.XMaxHallCorner;
                        
            //        }
            //        else
            //        {
            //            Debug.Write("ummm");
            //        }
            //    }

            //}

            //else
            //{
            //    if (p.X < Map.XMinCorner)
            //        p.X = Map.XMinCorner;
            //    else if (p.X > Map.ZMaxCorner)
            //        p.X = Map.ZMaxCorner;


            //}
            //PutThetaInPlace(p);
            if (!IsValidParticle(p))
            {
                //Debug.WriteLine("\t\tasssss");
                //Debug.WriteLine("****************** original: " + p1.ToString() + " modified: " + p.ToString());
               // Debug.WriteLine("*****putting it in bound again: " + p.ToString());
                PutParticlePoseInMap(p);
                //Debug.WriteLine("*****putting it in bound again: " + p.ToString());
            }
        }

        #endregion
      
    }
}